// ros2 topic pub /pause_robot std_msgs/msg/Bool "data: true"
// ros2 topic pub /pause_robot std_msgs/msg/Bool "data: false"
// 添加了arm_position_holder，实现碰撞急停恢复
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>

#include <std_msgs/msg/bool.hpp>
#include <controller_manager_msgs/srv/switch_controller.hpp>

#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#include <memory>
#include <atomic>
#include <functional> 

class CartesianExecutorNode : public rclcpp::Node
{
public:
    // 定义服务回调的类型别名
    using ServiceCallback = std::function<void(
        rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture
    )>;

    CartesianExecutorNode()
        : rclcpp::Node("cartesian_path_executor_node",
                       rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
    {
        // *** 关键配置：请修改为你自己的值 ***
        PLANNING_GROUP_NAME = "zc4"; // <--- 修改这里
        CSV_FILE_PATH = "/root/ws/waypoints_path2.csv"; // <--- 修改这里
        JTC_NAME = "joint_trajectory_controller";
        HOLDING_CONTROLLER_NAME = "arm_position_holder";
        // 笛卡尔路径参数
        eef_step_ = 0.01;
        jump_threshold_ = 0.0;
        avoid_collisions_ = true;
        
        RCLCPP_INFO(this->get_logger(), "配置: Group=%s, JTC=%s, Holder=%s",
            PLANNING_GROUP_NAME.c_str(), JTC_NAME.c_str(), HOLDING_CONTROLLER_NAME.c_str());

        // 初始化 MoveGroupInterface
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            std::shared_ptr<rclcpp::Node>(this), PLANNING_GROUP_NAME);
        
        RCLCPP_INFO(this->get_logger(), "MoveGroupInterface 初始化完成。");

        // 初始化暂停信号订阅者
        pause_subscriber_ = this->create_subscription<std_msgs::msg::Bool>(
            "/pause_robot",
            10,
            std::bind(&CartesianExecutorNode::pauseCallback, this, std::placeholders::_1)
        );

        // 初始化控制器切换服务客户端
        switch_controller_client_ = this->create_client<controller_manager_msgs::srv::SwitchController>(
            "/controller_manager/switch_controller"
        );
        
        is_paused_.store(false);
        // [FIX] 新增：初始化为 true
        is_initial_run_.store(true); 

        RCLCPP_INFO(this->get_logger(), "节点已准备就绪，等待控制器服务...");
        if (!switch_controller_client_->wait_for_service(std::chrono::seconds(5))) {
            RCLCPP_ERROR(this->get_logger(), "控制器切换服务 (/controller_manager/switch_controller) 不可用！");
        }
    }

    // run() 函数负责加载数据并触发 *初始* 执行
    void run()
    {
        // --- 1. 读取和解析CSV文件 ---
        RCLCPP_INFO(this->get_logger(), "正在从 %s 读取路径点...", CSV_FILE_PATH.c_str());
        original_waypoints_ = parseWaypointsFromCSV(CSV_FILE_PATH, this->get_logger());

        if (original_waypoints_.empty()) {
            RCLCPP_ERROR(this->get_logger(), "路径点列表为空，程序终止。");
            return;
        }

        RCLCPP_INFO(this->get_logger(), "航点加载完毕。");
        RCLCPP_INFO(this->get_logger(), "正在激活 JTC 以准备初始执行...");

        // --- 2. 激活 JTC 并开始执行 ---
        // (保持不变) onResumeSwitchResult 将处理 is_initial_run_ 逻辑
        callSwitchController(
            {HOLDING_CONTROLLER_NAME}, 
            {JTC_NAME},
            std::bind(&CartesianExecutorNode::onResumeSwitchResult, this, std::placeholders::_1)
        );
    }


private:
    // 暂停/继续 的回调函数 (逻辑更新)
    void pauseCallback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        bool should_pause = msg->data;

        if (should_pause && !is_paused_.load())
        {
            // --- 收到 PAUSE 信号 ---
            RCLCPP_WARN(this->get_logger(), "收到 PAUSE 信号！");
            is_paused_.store(true);
            
            // [FIX] 新增：一旦暂停，它就不再是“初始运行”
            is_initial_run_.store(false);

            // [关键] 立即停止 MoveIt 端的任何活动
            move_group_->stop(); 
            
            RCLCPP_INFO(this->get_logger(), "正在停止 JTC 并启动 Holding Controller...");
            callSwitchController(
                {JTC_NAME}, 
                {HOLDING_CONTROLLER_NAME},
                std::bind(&CartesianExecutorNode::onSwitchControllerResult, this, std::placeholders::_1)
            );
        }
        else if (!should_pause && is_paused_.load())
        {
            // --- 收到 RESUME 信号 ---
            RCLCPP_INFO(this->get_logger(), "收到 RESUME 信号！");
            is_paused_.store(false);
            
            // [FIX] 新增：恢复时，也绝不是“初始运行”
            is_initial_run_.store(false);

            RCLCPP_INFO(this->get_logger(), "正在停止 Holding Controller 并重启 JTC...");
            callSwitchController(
                {HOLDING_CONTROLLER_NAME}, 
                {JTC_NAME},
                std::bind(&CartesianExecutorNode::onResumeSwitchResult, this, std::placeholders::_1)
            );
        }
    }

    // [FIX] 核心逻辑：修改为区分“初始运行”和“恢复”
    void startOrResumeExecution()
    {
        if (is_paused_.load()) {
            RCLCPP_WARN(this->get_logger(), "startOrResumeExecution 被调用，但节点已暂停。中止。");
            return;
        }

        // --- 1. [关键] 告诉 MoveIt 从当前的实际状态开始规划 ---
        move_group_->setStartStateToCurrentState();
        
        std::vector<geometry_msgs::msg::Pose> remaining_waypoints;

        // [FIX] 检查我们是首次运行还是恢复
        if (is_initial_run_.load())
        {
            RCLCPP_INFO(this->get_logger(), "初始运行：规划完整路径...");
            // 是初始运行，使用 *所有* 航点
            remaining_waypoints = original_waypoints_;
            // 将标志设为 false，下一次（恢复时）将使用 'else' 逻辑
            is_initial_run_.store(false); 
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "恢复运行：从最近点规划剩余路径...");
            
            // --- 这是之前的逻辑：仅用于“恢复” ---
            auto current_pose_stamped = move_group_->getCurrentPose(move_group_->getEndEffectorLink());
            geometry_msgs::msg::Pose current_pose = current_pose_stamped.pose;

            size_t closest_index = 0;
            double min_dist_sq = -1.0;
            
            for (size_t i = 0; i < original_waypoints_.size(); ++i) {
                double dx = current_pose.position.x - original_waypoints_[i].position.x;
                double dy = current_pose.position.y - original_waypoints_[i].position.y;
                double dz = current_pose.position.z - original_waypoints_[i].position.z;
                double dist_sq = dx*dx + dy*dy + dz*dz;

                if (min_dist_sq < 0.0 || dist_sq < min_dist_sq) {
                    min_dist_sq = dist_sq;
                    closest_index = i;
                }
            }
            
            RCLCPP_INFO(this->get_logger(), "当前位置最接近原始航点索引: %zu (距离: %f m)", 
                closest_index, std::sqrt(min_dist_sq));

            remaining_waypoints.assign(
                original_waypoints_.begin() + closest_index, 
                original_waypoints_.end()
            );
        }
        

        // --- 3. (保留) 检查剩余航点 (现在“初始运行”不会在这里失败了)
        if (remaining_waypoints.size() <= 1) {
            // 这个日志现在只会在“恢复”时且恰好在最后一个点上，或“初始运行”时CSV文件有问题时触发
            RCLCPP_INFO(this->get_logger(), "没有剩余的航点可执行。路径完成。");
            return;
        }

        // --- 4. (保留) 为剩余的航点重新规划笛卡尔路径
        RCLCPP_INFO(this->get_logger(), "开始为 %zu 个航点重新规划笛卡尔路径...", remaining_waypoints.size());
        
        moveit_msgs::msg::RobotTrajectory remaining_trajectory;
        double fraction = move_group_->computeCartesianPath(
            remaining_waypoints, eef_step_, jump_threshold_, remaining_trajectory, avoid_collisions_
        );

        RCLCPP_INFO(this->get_logger(), "路径规划完成 (%.2f%% 成功)", fraction * 100.0);

        // --- 5. (保留) 异步执行新规划的轨迹
        if (fraction > 0.9)
        {
            moveit::planning_interface::MoveGroupInterface::Plan resume_plan;
            resume_plan.trajectory = remaining_trajectory;
            
            auto result = move_group_->asyncExecute(resume_plan);
            
            if (result != moveit::core::MoveItErrorCode::SUCCESS) {
                RCLCPP_ERROR(this->get_logger(), "轨迹异步执行启动失败: %d", result.val);
            } else {
                RCLCPP_INFO(this->get_logger(), "已开始执行轨迹...");
            }
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "未能为路径规划 (仅 %f 成功)。执行中止。", fraction);
        }
    }

    // [FIXED] callSwitchController 函数 (使用 BEST_EFFORT)
    void callSwitchController( const std::vector<std::string>& stop_controllers,
                               const std::vector<std::string>& start_controllers,
                               const ServiceCallback& callback )
    {
        auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
        request->activate_controllers = start_controllers;
        request->deactivate_controllers = stop_controllers;
        
        // 使用 BEST_EFFORT 避免启动时出错
        request->strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; 
        
        request->activate_asap = true;

        RCLCPP_DEBUG(this->get_logger(), "发送控制器切换请求...");
        switch_controller_client_->async_send_request(request, callback);
    }

    // (保留) 通用的切换服务结果回调
    void onSwitchControllerResult(
        rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture future)
    {
        try
        {
            auto result = future.get();
            if (result->ok) {
                RCLCPP_INFO(this->get_logger(), "控制器切换成功！");
            } else {
                RCLCPP_ERROR(this->get_logger(), "控制器切换失败！(result->ok == false)");
            }
        }
        catch (const std::exception& e)
        {
            RCLCPP_ERROR(this->get_logger(), "控制器切换服务调用失败: %s", e.what());
        }
    }

    // (保留) *专门用于恢复* 的回调函数
    void onResumeSwitchResult(
        rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture future)
    {
        try
        {
            auto result = future.get();
            if (result->ok) {
                RCLCPP_INFO(this->get_logger(), "JTC (重启) G! 准备执行路径...");
                
                // [关键] 在一个新线程中开始(或恢复)执行
                std::thread(&CartesianExecutorNode::startOrResumeExecution, this).detach();

            } else {
                RCLCPP_ERROR(this->get_logger(), "JTC 启动失败！无法执行路径。");
            }
        }
        catch (const std::exception& e)
        {
            RCLCPP_ERROR(this->get_logger(), "切换到 JTC 服务调用失败: %s", e.what());
        }
    }

    // (保留) CSV解析函数
    std::vector<geometry_msgs::msg::Pose> parseWaypointsFromCSV(
            const std::string& csv_file_path, 
            rclcpp::Logger logger);

    // --- 成员变量 ---
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pause_subscriber_;
    rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
    
    std::atomic<bool> is_paused_;
    // [FIX] 新增状态标志
    std::atomic<bool> is_initial_run_; 

    // 配置
    std::string PLANNING_GROUP_NAME;
    std::string CSV_FILE_PATH;
    std::string JTC_NAME;
    std::string HOLDING_CONTROLLER_NAME;

    // 路径参数
    double eef_step_;
    double jump_threshold_;
    bool avoid_collisions_;

    // 存储从CSV加载的 *所有* 航点
    std::vector<geometry_msgs::msg::Pose> original_waypoints_;
};


// -----------------------------------------------------------------
// (保留) 帮助函数：parseWaypointsFromCSV (与之前相同)
// -----------------------------------------------------------------
std::vector<geometry_msgs::msg::Pose> CartesianExecutorNode::parseWaypointsFromCSV(
    const std::string& csv_file_path, 
    rclcpp::Logger logger)
{
    std::vector<geometry_msgs::msg::Pose> waypoints;
    std::ifstream file(csv_file_path);

    if (!file.is_open()) {
        RCLCPP_ERROR(logger, "无法打开CSV文件: %s", csv_file_path.c_str());
        return waypoints;
    }

    std::string line;
    // std::getline(file, line); // 跳过表头

    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string value_str;
        std::vector<double> values;

        while (std::getline(ss, value_str, ',')) {
            try {
                values.push_back(std::stod(value_str));
            } catch (const std::invalid_argument& e) {
                RCLCPP_ERROR(logger, "CSV解析错误: '%s' 不是一个有效的数字", value_str.c_str());
                continue;
            }
        }

        if (values.size() != 7) {
            RCLCPP_WARN(logger, "CSV行格式错误。应为7个值 (x,y,z,qx,qy,qz,qw)，但只找到 %zu 个。跳过此行。", values.size());
            continue;
        }

        geometry_msgs::msg::Pose pose;
        pose.position.x = values[0];
        pose.position.y = values[1];
        pose.position.z = values[2];
        pose.orientation.x = values[3]; // qx
        pose.orientation.y = values[4]; // qy
        pose.orientation.z = values[5]; // qz
        pose.orientation.w = values[6]; // qw
        waypoints.push_back(pose);
    }

    file.close();
    RCLCPP_INFO(logger, "成功从CSV文件解析了 %zu 个路径点。", waypoints.size());
    return waypoints;
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    auto cartesian_executor = std::make_shared<CartesianExecutorNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(cartesian_executor);

    std::thread executor_thread([&executor]() {
        RCLCPP_INFO(rclcpp::get_logger("main"), "Spinner 线程启动...");
        executor.spin();
        RCLCPP_INFO(rclcpp::get_logger("main"), "Spinner 线程关闭。");
    });

    std::this_thread::sleep_for(std::chrono::seconds(2));

    RCLCPP_INFO(rclcpp::get_logger("main"), "调用 run() 来加载路径并开始执行...");
    cartesian_executor->run();

    executor_thread.join();
    
    rclcpp::shutdown();
    return 0;
}